#include "StdAfx.h"
#include <math.h>

#include "FFTWrapper.h"
#include "Randomizer.h"
#include "AmplitudeIndex.h"

static Randomizer randomizer;

FFT::FFT(){}

FFT::~FFT(){}

unsigned int FFT::GetArrayLength()
{
	return complexOut.size();
}

Complex*  FFT::DoFFT(short* sample, unsigned int length)
{
	if(complexOut.size()!=length)
	{
		Initialize(length);
	}
	short_to_complex(sample, &complexOut[0], length);
	FFTImpl(&complexOut[0], length, sinAr);
	return &complexOut[0];
}

void FFT::InvFFT()
{
	InvFFT(&complexOut[0], &shortOut[0], shortOut.size());
}

void FFT::InvFFT(Complex* complex, short* destination, unsigned int length)
{
	FFTImpl(complex, length, sinArNeg);
	complex_to_short(complex, destination, length);
}

/*double* FFT::GetRealIn()
{
	return &realIn[0];
}

Complex* FFT::GetComplexIn()
{
	return &complexIn[0];
}*/

Complex* FFT::GetComplexOut()
{
	return &complexOut[0];
}

/*double* FFT::GetRealOut()
{
	return &realOut[0];
}*/

short* FFT::GetShortOut()
{
	return &shortOut[0];
}

void FFT::Initialize(unsigned int length)
{
	//realIn.resize(length);
	//complexIn.resize(length);
	complexOut.resize(length);
	//realOut.resize(length);
	shortOut.resize(length);	

	cosAr.resize(length);
	sinAr.resize(length);
	sinArNeg.resize(length);
	bitRev.resize(length);

    register unsigned int i;	
	for(i=0; i<length; i++)
	{
		double arg = 2 * PI * i/length;
		cosAr[i] = cos(arg);
		sinAr[i] = sin(arg);
		sinArNeg[i] = -(sinAr[i]);
	}

	switch(length) {
		case 512:
			nu = 9;
			break;
		case 1024:
			nu = 10;
			break;
		case 2048:
			nu = 11;
			break;
		case 4096:
			nu = 12;
			break;
	}

	//log2
	//nu = length;
    //for (i = -1; nu!=0; i++, nu>>=1)
    //    ;
	
	//bitreverse
	register int j;
	for(i=0; i<length; i++)
	{
		unsigned int newVal = 0;
		unsigned int k = i;
		for(j=0; j<nu; j++)
		{
			newVal <<= 1;
			newVal |= k & 1;
			k >>= 1;
		}
		bitRev[i] = newVal;
	}
}

void FFT::FFTImpl(Complex* complex, unsigned int length, std::vector<double>& curSinAr)
{
	int dual_space = length;    /* Spacing between dual nodes */
    int nu1 = nu;				/* = nu-1 right shift needed when finding p */
    int k;                      /* Iteration of factor array */
    register int i;             /* Number of dual node pairs considered */
    register int j;             /* Index into factor array */
                
    /* For each iteration of factor matrix */
    for (k = 0; k < nu; k++) {
        /* Initialize */
        dual_space /= 2;        /* Fewer elements in each set of duals */
        nu1--;                  /* nu1 = nu - 1 */

        /* For each set of duals */
        for(j = 0; j < length; j += dual_space) {
            /* For each dual node pair */
            for (i = 0; i < dual_space; i++, j++) {
                double treal, timag;              /* Temp of w**p */
                register int p = bitRev[j >> nu1];
                                
                treal = complex[j+dual_space].r*cosAr[p] + complex[j+dual_space].i*curSinAr[p];
                timag = complex[j+dual_space].i*cosAr[p] - complex[j+dual_space].r*curSinAr[p];

                complex[j+dual_space].r = complex[j].r - treal;
                complex[j+dual_space].i = complex[j].i - timag;

                complex[j].r += treal;
                complex[j].i += timag;
            }
        }
    }

    /* We are done with the transform, now unscramble results */
    for (j = 0; j < length; j++) {
        if ((i = bitRev[j]) > j) {
            double treal, timag;

            /* Swap */
            treal = complex[j].r;
            timag = complex[j].i;

            complex[j].r = complex[i].r;
            complex[j].i = complex[i].i;

            complex[i].r = treal;
            complex[i].i = timag;
        }
    }
}

void FFT::Normalize(Complex* complex, unsigned int length)
{
    unsigned register int i;    
    for (i = 0; i < length; i++) {
        complex[i].r /= length;
        complex[i].i /= length;
    }
}


FFTConvolutionProcessor::FFTConvolutionProcessor(Complex* complex, unsigned int length)
{
	SetComplex(complex, length);
}

FFTConvolutionProcessor::~FFTConvolutionProcessor(){}

void FFTConvolutionProcessor::SetComplex(Complex* complex, unsigned int length)
{
	myBuf.resize(length);
	for(unsigned int i=0; i<length; i++)
	{
		myBuf[i] = complex[i];
	}
}

Complex* FFTConvolutionProcessor::Process(Complex* complex, unsigned int length, SortedAmplitudes* sortedAmplitudes)
{
	fft_convolution(&myBuf[0], complex, myBuf.size());
	return &myBuf[0];
}


FFTRandomSwapProcessor::FFTRandomSwapProcessor()
{
}

FFTRandomSwapProcessor::~FFTRandomSwapProcessor()
{
}

static std::vector<int> to;
static std::vector<int> from;

double min(double left, double right)
{
	return left <= right ? left : right;
}

double max(double left, double right)
{
	return left >= right ? left : right;
}

static void ChooseBins(unsigned int numbins)
{
	to.resize(numbins);
	from.resize(numbins);
	for(unsigned int i=0; i<numbins; i++)
	{
		to[i] = i;
	}

	for (unsigned int i=0; i<numbins; ++i) {
		int j = (int) (randomizer.NextPositiveDouble()  * (numbins - i));
		int temp = to[i];
		to[i] = to[j];
		to[j] = temp;
	}

	int width = (int) (0.49 * numbins);//(int32)(ZIN0(2) * numbins);
	for (unsigned int i=0; i<numbins; ++i) {
		int k = to[i];
		int minr = std::max(0, k-width);
		int maxr = (int)(min(numbins-1, k+width));
		int j = (int) (randomizer.NextPositiveDouble() * (maxr - minr) + minr);
		from[i] = j;
	}
}

Complex* FFTRandomSwapProcessor::Process(Complex* complex, unsigned int length, SortedAmplitudes* sortedAmplitudes)
{
	bool resized = false;
	int numbins = length/2;
	if(myBuf.size() != length)
	{
		myBuf.resize(length);
		ChooseBins(numbins);
		resized = true;
	}
	for(unsigned int i=0; i<length; i++)
	{
		myBuf[i] = complex[i];
	}
	
	double wipe = 0.49;//ZIN0(1);
	int scrambleBins = (int)(numbins * max( min(wipe, 1.f), 0.f));

	for (int j=0; j<scrambleBins; j++) {
		myBuf[to[j]] = complex[from[j]];
	}
	for (int j=scrambleBins; j<numbins; j++) {
		int a = to[j];
		myBuf[a] = complex[a];
	}


	return &myBuf[0];
}

FFTLowPassProcessor::FFTLowPassProcessor(unsigned int threshold)
{
	this->threshold = threshold;
}
FFTLowPassProcessor::~FFTLowPassProcessor(){}

Complex* FFTLowPassProcessor::Process(Complex* complex, unsigned int len, SortedAmplitudes* sortedAmplitudes)
{
	unsigned int half = len/2;
	if(myBuf.size() != len)
	{
		myBuf.resize(len);
	}
	register unsigned int i=0;
	for(; i<threshold; i++)
	{
		myBuf[i] = complex[i];
	}
	for(; i<len-threshold; i++)
	{
		myBuf[i].i = 0;
		myBuf[i].r = 0;
	}	
	for(; i<len; i++)
	{
		myBuf[i] = complex[i];
	}

	return &myBuf[0];
}

FFTHiPassProcessor::FFTHiPassProcessor(unsigned int threshold)
{
	this->threshold = threshold;
}
FFTHiPassProcessor::~FFTHiPassProcessor(){}

Complex* FFTHiPassProcessor::Process(Complex* complex, unsigned int len, SortedAmplitudes* sortedAmplitudes)
{
	unsigned int half = len/2;
	if(myBuf.size() != len)
	{
		myBuf.resize(len);
	}
	register unsigned int i=0;
	for(; i<threshold; i++)
	{
		myBuf[i].i = 0;
		myBuf[i].r = 0;
	}
	for(; i<len-threshold; i++)
	{
		myBuf[i] = complex[i];
	}	
	for(; i<len; i++)
	{
		myBuf[i].i = 0;
		myBuf[i].r = 0;
	}

	return &myBuf[0];
}

FFTBandPassProcessor::FFTBandPassProcessor(unsigned int startIdx, unsigned int endIdx)
{
	this->startIdx = startIdx;
	this->endIdx = endIdx;
}
FFTBandPassProcessor::~FFTBandPassProcessor(){}

Complex* FFTBandPassProcessor::Process(Complex* complex, unsigned int len, SortedAmplitudes* sortedAmplitudes)
{
	unsigned int half = len/2;
	if(myBuf.size() != len)
	{
		myBuf.resize(len);
	}
	register unsigned int i=0;
	for(; i<startIdx; i++)
	{
		myBuf[i].i = 0;
		myBuf[i].r = 0;
	}
	for(; i<endIdx; i++)
	{
		myBuf[i] = complex[i];
	}
	for(; i<half/*+endIdx*/; i++)
	{
		myBuf[i].i = 0;
		myBuf[i].r = 0;
	}	
	/*
	for(; i<len-startIdx; i++)
	{
		myBuf[i] = complex[i];
	}
	for(; i<len; i++)
	{
		myBuf[i].i = 0;
		myBuf[i].r = 0;
	}
	*/

	return &myBuf[0];
}

FFTShiftUpProcessor::FFTShiftUpProcessor(unsigned int amount)
{
	this->amount = amount;
}
FFTShiftUpProcessor::~FFTShiftUpProcessor(){}

Complex* FFTShiftUpProcessor::Process(Complex* complex, unsigned int length, SortedAmplitudes* sortedAmplitudes)
{
	if(myBuf.size() != length)
	{
		myBuf.resize(length);
	}
	unsigned int half = length/2;
	unsigned int i=0;
	for(; i<amount; i++)
	{
		myBuf[i].r = 0;
		myBuf[i].i = 0;
	}
	for(; i<half; i++)
	{
		myBuf[i] = complex[i-amount];
	}
	for(; i<half+amount; i++)
	{
		myBuf[i] = complex[i+amount];
	}
	for(; i<length; i++)
	{
		myBuf[i].r = 0;
		myBuf[i].i = 0;
	}
	return &myBuf[0];
}

FFTShiftDownProcessor::FFTShiftDownProcessor(unsigned int amount)
{
	this->amount = amount;
}

FFTShiftDownProcessor::~FFTShiftDownProcessor(){}

Complex* FFTShiftDownProcessor::Process(Complex* complex, unsigned int length, SortedAmplitudes* sortedAmplitudes)
{
	if(myBuf.size() != length)
	{
		myBuf.resize(length);
	}
	unsigned int half = length/2;
	unsigned int i=0;
	for(; i<amount; i++)
	{
		myBuf[i] = complex[i+amount];
	}
	for(; i<half+amount; i++)
	{
		myBuf[i].r = 0;
		myBuf[i].i = 0;
	}
	for(; i<length; i++)
	{
		myBuf[i] = complex[i-amount];
	}
	return &myBuf[0];
}

FFTBinMaximizer::FFTBinMaximizer(unsigned int* bins, unsigned int length)
{
	if(bins)
	{
		for(unsigned int i=0; i<length; i++)
		{
			this->bins.push_back(bins[i]);
		}
	}
}

FFTBinMaximizer::~FFTBinMaximizer(){}

Complex* FFTBinMaximizer::Process(Complex* complex, unsigned int length, SortedAmplitudes* sortedAmplitudes)
{
	if(myBuf.size() != length)
	{
		myBuf.resize(length);
	}
	for(unsigned int i=0; i<length; i++)
	{
		myBuf[i] = complex[i];
	}

	double max = 0;
	for(unsigned int i=0; i<length/2; i++)
	{
		if(complex[i].r > max)
			max = complex[i].r;
	}
	for(unsigned int i=0; i<bins.size(); i++)
	{
		unsigned int bin = bins[i];
		myBuf[bin].r = max;
		myBuf[bin].i = 0;
	}

	return &myBuf[0];
}

FFTBinDampener::FFTBinDampener(unsigned int untouchedCount)
{
	this->untouchedCount = untouchedCount;
}

FFTBinDampener::~FFTBinDampener(){}

Complex* FFTBinDampener::Process(Complex* complex, unsigned int length, SortedAmplitudes* sortedAmplitudes)
{
	if(myBuf.size() != length)
	{
		myBuf.resize(length);
	}
	for(unsigned int i=0; i<length; i++)
	{
		if(complex[i].r < 0.001 && complex[i].r > -0.001)
		{
			myBuf[i].r = complex[i].r * 0.125;			
		}
		else
		{
			myBuf[i].r = complex[i].r;
		}
		myBuf[i].i = complex[i].i;
	}

	/*
	std::vector<AmplitudeIndex> amps;
	sortedAmplitudes->GetHighestAmplitudes(untouchedCount, amps);
	for(unsigned int i=0; i<amps.size(); i++)
	{
		unsigned int bin = amps[i].index;
		myBuf[bin].r *= 8;		
	}
	*/
	return &myBuf[0];
}

